package model

import (
	"encoding/xml"
	"fmt"
	"math"
	"strconv"
	"strings"

	"dev.ixmxm.top/rms/utils/tools"

	"dev.ixmxm.top/rms/codes"
	"dev.ixmxm.top/rms/utils/angles"
	"dev.ixmxm.top/rms/utils/base64"
	"dev.ixmxm.top/rms/utils/log"
)

//ParseMap 地图解析，由xml转json
func ParseMap(xmlRaw string) (*MapMetaData, *MapXml, error) {
	rawMetadata := &MapXml{}
	if err := xml.Unmarshal([]byte(xmlRaw), rawMetadata); err != nil {
		log.Panicf("parse xml map data error:%s", err.Error())
		return nil, nil, err
	}

	mapData := new(MapMetaData)
	var mapInfo MapInfo
	mapInfo.MapName = rawMetadata.MapInfo.MapName
	mapInfo.ProjectId = rawMetadata.MapInfo.ProjectId
	mapInfo.TrafficName = rawMetadata.MapInfo.TrafficName
	mapData.MapInfo = mapInfo
	mapData.NodesMap = make(map[string]Node, 0)
	mapData.Paths = make(map[string]Path, 0)
	mapData.Links = make(map[string]Link, 0)
	mapData.Areas = make(map[string]Area, 0)
	mapData.Obstacle = make(map[string]Obstacle, 0)
	mapData.MarkArea = make(map[string]MarkArea, 0)

	for _, n := range rawMetadata.Dots {
		dotsStr := strings.Split(n.BanDotID, ";")
		bans := make([]string, 0)
		for _, dotStr := range dotsStr {
			if dotStr == "" || dotStr == "0" {
				continue
			}
			bans = append(bans, dotStr)
		}

		associatedMutexId := ""
		//添加互斥区域
		for _, area := range rawMetadata.Areas {
			if AreaType(area.Type) == AreaTypeMutex || AreaType(area.Type) == AreaTypeTaskLimitRobotArea {
				// isAny := false
				for _, siteId := range area.ContainDotIds {
					if siteId == n.Id {
						associatedMutexId = area.Id
						break
					}
				}
			}

		}

		mapData.NodesMap[n.Id] = Node{
			Name:              n.Name,
			Id:                n.Id,
			Type:              n.Type,
			LocationType:      n.LocationType,
			X:                 n.X,
			Y:                 n.Y,
			DeviceID:          n.DeviceCode,
			BanNodes:          bans,
			IntermediateNode:  n.TransmitPoint,
			AssociatedMutexId: associatedMutexId,
			// AssociatedBlocks: associatedNodeBlocks,
			ForkRotateFlag: n.ForkRotateFlag,
			BanDir:         n.BanDir,
			AllowAvoid:     n.AllowAvoid,
			Reserve1:       n.Reserve1,
			Reserve2:       n.Reserve2,
		}
	}

	for i := 0; i < len(rawMetadata.Lines); i++ {
		nodeA, find := mapData.NodesMap[rawMetadata.Lines[i].SiteIdA]
		if !find {
			panic("line node A not found")
		}
		nodeB, find := mapData.NodesMap[rawMetadata.Lines[i].SiteIdB]
		if !find {
			panic("line node B not found")
		}

		makePath := func(dir PathDirection) Path {
			id_int, _ := strconv.Atoi(rawMetadata.Lines[i].Id)
			return Path{
				PathId:     rawMetadata.Lines[i].Id,
				Id:         id_int,
				Name:       rawMetadata.Lines[i].Name,
				Type:       PathTypeLine,
				Dir:        dir,
				BodyDirec:  rawMetadata.Lines[i].BodyDir,
				NodeA:      nodeA,
				NodeB:      nodeB,
				PathLength: rawMetadata.Lines[i].PathLength,
				PathVel:    float64(rawMetadata.Lines[i].Speed),
				AllowBack:  rawMetadata.Lines[i].AllowBack,
				Reserve1:   rawMetadata.Lines[i].Reserve1,
				Reserve2:   rawMetadata.Lines[i].Reserve2,
			}
		}
		switch rawMetadata.Lines[i].PathDir {
		case DirectionAB:
			mapData.Paths[nodeA.Id+"-"+nodeB.Id] = makePath(DirectionA2B)
			mapData.Paths[nodeB.Id+"-"+nodeA.Id] = makePath(DirectionB2A)
		case DirectionA2B:
			mapData.Paths[nodeA.Id+"-"+nodeB.Id] = makePath(DirectionA2B)
		case DirectionB2A:
			mapData.Paths[nodeB.Id+"-"+nodeA.Id] = makePath(DirectionB2A)
		}
	}

	//链接线赋值
	for i := 0; i < len(rawMetadata.Links); i++ {
		nodeA, find := mapData.NodesMap[rawMetadata.Links[i].SiteIdA]
		if !find {
			log.Error("link node A not found")
			continue
		}
		nodeB, find := mapData.NodesMap[rawMetadata.Links[i].SiteIdB]
		if !find {
			log.Error("link node B not found")
			continue
		}

		link := Link{
			LinkId:   rawMetadata.Links[i].Id,
			Name:     rawMetadata.Links[i].Name,
			Type:     PathType(rawMetadata.Links[i].Type),
			Dir:      rawMetadata.Links[i].PathDir,
			NodeA:    nodeA,
			NodeB:    nodeB,
			Reserve1: rawMetadata.Lines[i].Reserve1,
			Reserve2: rawMetadata.Lines[i].Reserve2,
		}
		mapData.Links[link.LinkId] = link

	}

	for i := 0; i < len(rawMetadata.BezierCurves); i++ {
		nodeA, find := mapData.NodesMap[rawMetadata.BezierCurves[i].SiteIdA]
		if !find {
			log.Panicf("line node A not found:%d", rawMetadata.BezierCurves[i].SiteIdA)
		}
		nodeB, find := mapData.NodesMap[rawMetadata.BezierCurves[i].SiteIdB]
		if !find {
			log.Panicf("line node B not found:%s", rawMetadata.BezierCurves[i].SiteIdB)
		}
		makePath := func(dir PathDirection) Path {
			id_int, _ := strconv.Atoi(rawMetadata.BezierCurves[i].Id)
			return Path{
				PathId:        rawMetadata.BezierCurves[i].Id,
				Id:            id_int,
				Name:          rawMetadata.BezierCurves[i].Name,
				Type:          PathTypeBezier,
				Dir:           dir,
				BodyDirec:     rawMetadata.BezierCurves[i].BodyDir,
				NodeA:         nodeA,
				NodeB:         nodeB,
				NodeAControlX: float64(rawMetadata.BezierCurves[i].AdjustPointAX),
				NodeAControlY: float64(rawMetadata.BezierCurves[i].AdjustPointAY),
				NodeBControlX: float64(rawMetadata.BezierCurves[i].AdjustPointBX),
				NodeBControlY: float64(rawMetadata.BezierCurves[i].AdjustPointBY),
				PathLength:    rawMetadata.BezierCurves[i].PathLength,
				PathVel:       float64(rawMetadata.BezierCurves[i].Speed),
				AllowBack:     rawMetadata.BezierCurves[i].AllowBack,
			}
		}
		switch rawMetadata.BezierCurves[i].PathDir {
		case DirectionAB:
			mapData.Paths[nodeA.Id+"-"+nodeB.Id] = makePath(DirectionA2B)
			mapData.Paths[nodeB.Id+"-"+nodeA.Id] = makePath(DirectionB2A)
		case DirectionA2B:
			mapData.Paths[nodeA.Id+"-"+nodeB.Id] = makePath(DirectionA2B)
		case DirectionB2A:
			mapData.Paths[nodeB.Id+"-"+nodeA.Id] = makePath(DirectionB2A)
		}
		// p := makePath(DirectionA2B)
		// poseList := p.ComputeBezierPolygon(1100.0, 600.0, 2100.0)
		// for idx, pose := range poseList {
		// 	log.Warnf("%d (%f,%f)", idx, pose.X, pose.Y)
		// }
	}
	//验证中转点是否合法
	for _, point := range mapData.NodesMap {
		if point.IntermediateNode != "" {
			intermediateNode, find := mapData.NodesMap[point.IntermediateNode]
			if !find {
				point.IntermediateNode = ""
			} else if intermediateNode.Type != NodeTransitPoint {
				point.IntermediateNode = ""
			}

		}
		res := make([]string, 0)
		for _, l := range mapData.Paths {
			if l.NodeA.Id == point.Id || l.NodeB.Id == point.Id {
				str := l.NodeA.Id + "-" + l.NodeB.Id
				if l.Dir == DirectionB2A {
					str = l.NodeB.Id + "-" + l.NodeA.Id
				}
				res = append(res, str)
			}
		}
		point.ConnectPaths = res
		mapData.NodesMap[point.Id] = point
	}
	for key, l := range mapData.Paths {
		l.NodeA = mapData.NodesMap[l.NodeA.Id]
		l.NodeB = mapData.NodesMap[l.NodeB.Id]
		mapData.Paths[key] = l
	}
	pathInList := func(id string, areaPath []Path) bool {
		for _, p := range areaPath {
			if p.PathId == id {
				return true
			}
		}
		return false
	}
	findPath := func(cntNodeId string, areaPath []Path, pathIds []string) (bool, Path) {
		for _, p := range mapData.Paths {
			if p.GetSource().Id == cntNodeId {
				for _, id := range pathIds {
					if id == p.PathId && !pathInList(id, areaPath) {
						return true, p
					}
				}
			}
		}
		return false, Path{}
	}
	for i := 0; i < len(rawMetadata.Areas); i++ {
		containPathIds := make([]Path, 0)
		if rawMetadata.Areas[i].Type == int(AreaTypeTide) {
			areaPath := make([]Path, 0)
			containPathIdsTemp := rawMetadata.Areas[i].ContainPathIds
			cntNodeId := rawMetadata.Areas[i].TideStartDotId
			endNodeId := rawMetadata.Areas[i].TideEndDotId
			if cntNodeId != "0" && endNodeId != "0" {
				for i := 0; i < 2*len(containPathIdsTemp); i++ {
					if len(areaPath) != 0 {
						cntNodeId = areaPath[len(areaPath)-1].GetTarget().Id
					}
					if cntNodeId == endNodeId {
						log.Warnf("潮汐区域路径拼接完成")
						containPathIds = areaPath
						break
					}
					find, p := findPath(cntNodeId, areaPath, containPathIdsTemp)
					if find {
						areaPath = append(areaPath, p)
					} else {
						log.Errorf("潮汐区域路径无法拼接")
						break
					}
				}
			} else {
				log.Errorf("潮汐区域路径起点或者终点id非法")
			}

		}
		area := Area{
			Id:              rawMetadata.Areas[i].Id,
			Name:            rawMetadata.Areas[i].Name,
			Type:            AreaType(rawMetadata.Areas[i].Type),
			EdgePoints:      rawMetadata.Areas[i].EdgePoints,
			BanAgvIds:       rawMetadata.Areas[i].BanAgvIds,
			ContainDotIds:   rawMetadata.Areas[i].ContainDotIds,
			ContainPathIds:  containPathIds,
			FrontLookPoints: rawMetadata.Areas[i].FrontLookPoints,
			AreaWork:        false,
			AgvList:         make([]string, 0),
			AlowRotate:      rawMetadata.Areas[i].AlowRotate,
			IsApply:         rawMetadata.Areas[i].IsApply,
			RobotLimitCount: rawMetadata.Areas[i].RobotLimitCount,
		}
		mapData.Areas[area.Id] = area
	}
	for _, rawMarkArea := range rawMetadata.MarkAreas {
		markArea := MarkArea{
			Id:         rawMarkArea.Id,
			Name:       rawMarkArea.Name,
			Encode:     rawMarkArea.EnCode,
			EdgePoints: rawMarkArea.EdgePoints,
		}
		mapData.MarkArea[markArea.Id] = markArea
	}

	for _, rawObstacle := range rawMetadata.Obstacles {
		obstacle := Obstacle{
			Id:         rawObstacle.Id,
			Name:       rawObstacle.Name,
			Type:       ObstacleType(rawObstacle.Type),
			EdgePoints: rawObstacle.EdgePoints,
		}
		mapData.Obstacle[obstacle.Id] = obstacle
	}
	// mapData.HandleMapAdjustFrontAGV()
	return mapData, rawMetadata, nil
}
func (mapData *MapMetaData) ParseMapNodeAssociatedNodeBlocks() {
	for key, n := range mapData.NodesMap {
		if n.AssociatedMutexId == "" {
			continue
		}
		associatedNodeBlocks := make([]string, 0)
		//添加互斥区域
		area, find := mapData.Areas[n.AssociatedMutexId]
		if !find {
			continue
		}
		if n.Id == "10571" {
			log.Infof("test")
		}
		associatedNodeBlocks = append(associatedNodeBlocks, area.ContainDotIds...)
		n.AssociatedBlocks = associatedNodeBlocks
		//赋值
		mapData.NodesMap[key] = n

	}
}

// func (p Path) ComputeBezierPolygon(width, radius_r, radius_R float64) []Pose2 {
// 	//P点在坐标系X'OY'中的坐标（x',y'）,可以求得点P在基坐标系XOY中的坐标值
// 	//x=x'·cos(-θ)+y'·sin(-θ)
// 	//y=y'·cos(-θ)-x'·sin(-θ)
// 	computeNewPose := func(point Pose2, theta, xn, yn float64) Pose2 {
// 		var pose Pose2
// 		pose.X = point.X + xn*math.Cos(-theta) + yn*math.Sin(-theta)
// 		pose.Y = point.Y + yn*math.Cos(-theta) - xn*math.Sin(-theta)
// 		return pose
// 	}
// 	calculateBezierPointForCubic := func(t float64, p0, p1, p2, p3 Pose2) Pose2 {
// 		temp := 1 - t
// 		var point Pose2
// 		point.X = p0.X*temp*temp*temp + 3*p1.X*t*temp*temp + 3*p2.X*t*t*temp + p3.X*t*t*t
// 		point.Y = p0.Y*temp*temp*temp + 3*p1.Y*t*temp*temp + 3*p2.Y*t*t*temp + p3.Y*t*t*t
// 		return point
// 	}
// 	poseList := make([]Pose2, 0)
// 	var pointS = Pose2{X: float64(p.NodeA.X), Y: float64(p.NodeA.Y)}
// 	var pointSc = Pose2{X: p.NodeAControlX, Y: p.NodeAControlY}
// 	var pointTc = Pose2{X: p.NodeBControlX, Y: p.NodeBControlY}
// 	var pointT = Pose2{X: float64(p.NodeB.X), Y: float64(p.NodeB.Y)}
// 	if p.BodyDirec == HeadBA || (p.BodyDirec == Head && p.Dir == DirectionB2A) {
// 		pointS = Pose2{X: float64(p.NodeB.X), Y: float64(p.NodeB.Y)}
// 		pointSc = Pose2{X: p.NodeBControlX, Y: p.NodeBControlY}
// 		pointTc = Pose2{X: p.NodeAControlX, Y: p.NodeAControlY}
// 		pointT = Pose2{X: float64(p.NodeA.X), Y: float64(p.NodeA.Y)}
// 	}
// 	pointM := calculateBezierPointForCubic(0.5, pointS, pointSc, pointTc, pointT)
// 	pointM1 := calculateBezierPointForCubic(0.51, pointS, pointSc, pointTc, pointT)
// 	angleS := math.Atan2(pointSc.Y-pointS.Y, pointSc.X-pointS.X)
// 	angleT := math.Atan2(pointT.Y-pointTc.Y, pointT.X-pointTc.X)
// 	angleM := math.Atan2(pointM1.Y-pointM.Y, pointM1.X-pointM.X)
// 	angleSM := math.Atan2(pointM.Y-pointS.Y, pointM.X-pointS.X)

// 	dis := math.Hypot(pointT.Y-pointTc.Y, pointT.X-pointTc.X)
// 	Ptm := computeNewPose(pointT, angleT, (radius_R-dis)/2, 0)
// 	ratio := math.Hypot(pointS.Y-pointSc.Y, pointS.X-pointSc.X) / radius_R
// 	if ratio < 1.0 {
// 		ratio = 1.0
// 	}
// 	if (angles.NormalizeAngle(angleS-angleM) > 0 && angles.NormalizeAngle(angleS-angleSM) > 0 && angles.NormalizeAngle(angleM-angleT) < 0) ||
// 		(angles.NormalizeAngle(angleS-angleM) < 0 && angles.NormalizeAngle(angleS-angleSM) < 0 && angles.NormalizeAngle(angleM-angleT) > 0) {
// 		ratio = (ratio-1.0)*0.75 + 1.3
// 	}
// 	ratio1 := math.Hypot(pointTc.X-pointT.X, pointTc.Y-pointT.Y) / radius_R
// 	if (angles.NormalizeAngle(angleM-angleT) > 0 && angles.NormalizeAngle(angleS-angleM) < 0) ||
// 		(angles.NormalizeAngle(angleM-angleT) < 0 && angles.NormalizeAngle(angleS-angleM) > 0) {
// 		if ratio1 < 1.0 {
// 			ratio1 = 1.0
// 		}
// 		ratio1 = (ratio1-1.0)*0.5 + 1.0
// 	}
// 	widthTemp := 0.75 * width

// 	poseList = append(poseList, computeNewPose(pointS, angleS, -radius_r, width/2))
// 	if math.Abs(angles.NormalizeAngle(angleS-angleM)) > math.Pi/90 || math.Abs(angles.NormalizeAngle(angleT-angleM)) > math.Pi/90 {
// 		if angles.NormalizeAngle(angleS-angleM) > 0 && angles.NormalizeAngle(angleS-angleSM) > 0 {
// 			poseList = append(poseList, computeNewPose(pointS, angleS, ratio*radius_R, width/2))

// 		} else {
// 			poseList = append(poseList, computeNewPose(pointS, angleS, 0, width/2))
// 		}
// 		if angles.NormalizeAngle(angleS-angleM) < 0 || angles.NormalizeAngle(angleM-angleT) < 0 {
// 			if angles.NormalizeAngle(angleS-angleM) > 0 {
// 				widthTemp = 1.2 * width
// 			}
// 			poseList = append(poseList, computeNewPose(pointM, angleM, 0, widthTemp))
// 		}
// 		if angles.NormalizeAngle(angleM-angleT) > 0 {
// 			P0 := poseList[len(poseList)-1]
// 			P1 := computeNewPose(pointT, angleT, -ratio1*radius_R, width/2)
// 			P2 := computeNewPose(Ptm, angleT, 0, 0.8*radius_R)
// 			theta1 := math.Atan2(P1.Y-P0.Y, P1.X-P0.X)
// 			theta2 := math.Atan2(P2.Y-P0.Y, P2.X-P0.X)
// 			if angles.NormalizeAngle(theta1-theta2) > 0 {
// 				poseList = append(poseList, P1)
// 			}
// 			poseList = append(poseList, P2)
// 		} else {
// 			poseList = append(poseList, computeNewPose(pointT, angleT, 0, width/2))
// 		}
// 	}

// 	poseList = append(poseList, computeNewPose(pointT, angleT, radius_R, width/2))
// 	poseList = append(poseList, computeNewPose(pointT, angleT, radius_R, -width/2))
// 	if math.Abs(angles.NormalizeAngle(angleS-angleM)) > math.Pi/90 || math.Abs(angles.NormalizeAngle(angleT-angleM)) > math.Pi/90 {
// 		if angles.NormalizeAngle(angleM-angleT) < 0 {
// 			P0 := computeNewPose(pointS, angleS, 0, -width/2)
// 			if angles.NormalizeAngle(angleS-angleM) < 0 && angles.NormalizeAngle(angleS-angleSM) < 0 {
// 				P0 = computeNewPose(pointS, angleS, ratio*radius_R, -width/2)
// 			}
// 			P1 := computeNewPose(Ptm, angleT, 0, -0.8*radius_R)
// 			P2 := computeNewPose(pointT, angleT, -ratio1*radius_R, -width/2)
// 			theta1 := math.Atan2(P1.Y-P0.Y, P1.X-P0.X)
// 			theta2 := math.Atan2(P2.Y-P0.Y, P2.X-P0.X)

// 			poseList = append(poseList, P1)
// 			if angles.NormalizeAngle(theta1-theta2) > 0 {
// 				poseList = append(poseList, P2)
// 			}
// 		} else {
// 			poseList = append(poseList, computeNewPose(pointT, angleT, 0, -width/2))
// 		}
// 		if angles.NormalizeAngle(angleS-angleM) > 0 || angles.NormalizeAngle(angleM-angleT) > 0 {
// 			if angles.NormalizeAngle(angleS-angleM) < 0 {
// 				widthTemp = 1.2 * width
// 			}
// 			poseList = append(poseList, computeNewPose(pointM, angleM, 0, -widthTemp))
// 		}
// 		if angles.NormalizeAngle(angleS-angleM) < 0 && angles.NormalizeAngle(angleS-angleSM) < 0 {
// 			poseList = append(poseList, computeNewPose(pointS, angleS, ratio*radius_R, -width/2))
// 		} else {
// 			poseList = append(poseList, computeNewPose(pointS, angleS, 0, -width/2))
// 		}
// 	}
// 	poseList = append(poseList, computeNewPose(pointS, angleS, -radius_r, -width/2))

// 	return poseList
// }
func (p Path) ComputeBezierPolygon(width, radius_r, radius_R float64) []Pose2 {
	//P点在坐标系X'OY'中的坐标（x',y'）,可以求得点P在基坐标系XOY中的坐标值
	//x=x'·cos(-θ)+y'·sin(-θ)
	//y=y'·cos(-θ)-x'·sin(-θ)
	computeNewPose := func(point Pose2, theta, xn, yn float64) Pose2 {
		var pose Pose2
		pose.X = point.X + xn*math.Cos(-theta) + yn*math.Sin(-theta)
		pose.Y = point.Y + yn*math.Cos(-theta) - xn*math.Sin(-theta)
		return pose
	}
	calculateBezierPointForCubic := func(t float64, p0, p1, p2, p3 Pose2) Pose2 {
		temp := 1 - t
		var point Pose2
		point.X = p0.X*temp*temp*temp + 3*p1.X*t*temp*temp + 3*p2.X*t*t*temp + p3.X*t*t*t
		point.Y = p0.Y*temp*temp*temp + 3*p1.Y*t*temp*temp + 3*p2.Y*t*t*temp + p3.Y*t*t*t
		return point
	}
	poseList := make([]Pose2, 0)
	var pointS = Pose2{X: float64(p.NodeA.X), Y: float64(p.NodeA.Y)}
	var pointSc = Pose2{X: p.NodeAControlX, Y: p.NodeAControlY}
	var pointTc = Pose2{X: p.NodeBControlX, Y: p.NodeBControlY}
	var pointT = Pose2{X: float64(p.NodeB.X), Y: float64(p.NodeB.Y)}
	if p.BodyDirec == HeadBA || (p.BodyDirec == Head && p.Dir == DirectionB2A) {
		pointS = Pose2{X: float64(p.NodeB.X), Y: float64(p.NodeB.Y)}
		pointSc = Pose2{X: p.NodeBControlX, Y: p.NodeBControlY}
		pointTc = Pose2{X: p.NodeAControlX, Y: p.NodeAControlY}
		pointT = Pose2{X: float64(p.NodeA.X), Y: float64(p.NodeA.Y)}
	}
	pointM := calculateBezierPointForCubic(0.5, pointS, pointSc, pointTc, pointT)
	pointM1 := calculateBezierPointForCubic(0.51, pointS, pointSc, pointTc, pointT)
	pointSM := calculateBezierPointForCubic(0.25, pointS, pointSc, pointTc, pointT)
	pointSM1 := calculateBezierPointForCubic(0.26, pointS, pointSc, pointTc, pointT)
	pointMT := calculateBezierPointForCubic(0.75, pointS, pointSc, pointTc, pointT)
	pointMT1 := calculateBezierPointForCubic(0.76, pointS, pointSc, pointTc, pointT)
	angleS := math.Atan2(pointSc.Y-pointS.Y, pointSc.X-pointS.X)
	angleT := math.Atan2(pointT.Y-pointTc.Y, pointT.X-pointTc.X)
	angleM := math.Atan2(pointM1.Y-pointM.Y, pointM1.X-pointM.X)
	angleSM := math.Atan2(pointSM1.Y-pointSM.Y, pointSM1.X-pointSM.X)
	angleMT := math.Atan2(pointMT1.Y-pointMT.Y, pointMT1.X-pointMT.X)

	poseList = append(poseList, computeNewPose(pointS, angleS, -radius_r, width/2))
	if angles.NormalizeAngle(angleM-angleS) > 0.0 {
		poseList = append(poseList, computeNewPose(pointSM, angleSM, 0, width/2))
	} else {
		poseList = append(poseList, computeNewPose(pointS, angleS, radius_R, width/2))
		poseList = append(poseList, computeNewPose(pointSM, angleSM, radius_R, width/2))
	}
	if angles.NormalizeAngle(angleT-angleM) > 0.0 {
		if angles.NormalizeAngle(angleM-angleS) > 0.0 {
			poseList = append(poseList, computeNewPose(pointM, angleM, 0, width/2))
		}
		poseList = append(poseList, computeNewPose(pointT, angleT, 0, width/2))
	} else {
		poseList = append(poseList, computeNewPose(pointM, angleM, radius_R, width/2))
		poseList = append(poseList, computeNewPose(pointMT, angleMT, radius_R, width/2))
	}
	poseList = append(poseList, computeNewPose(pointT, angleT, radius_R, width/2))
	poseList = append(poseList, computeNewPose(pointT, angleT, radius_R, -width/2))
	if angles.NormalizeAngle(angleT-angleM) > 0.0 {
		poseList = append(poseList, computeNewPose(pointMT, angleMT, radius_R, -width/2))
		poseList = append(poseList, computeNewPose(pointM, angleM, radius_R, -width/2))

	} else {
		poseList = append(poseList, computeNewPose(pointT, angleT, 0, -width/2))
		if angles.NormalizeAngle(angleM-angleS) < 0.0 {
			poseList = append(poseList, computeNewPose(pointM, angleM, 0, -width/2))
		}
	}
	if angles.NormalizeAngle(angleM-angleS) > 0.0 {
		poseList = append(poseList, computeNewPose(pointSM, angleSM, radius_R, -width/2))
		poseList = append(poseList, computeNewPose(pointS, angleS, radius_R, -width/2))
	} else {
		poseList = append(poseList, computeNewPose(pointSM, angleSM, 0, -width/2))
	}
	poseList = append(poseList, computeNewPose(pointS, angleS, -radius_r, -width/2))
	return poseList
}

type MapSaveType uint8

const (
	MapSaveTypeMapDataNodes MapSaveType = 0 //保存点和基础数据
	MapSaveTypeMapDataPath  MapSaveType = 1 //保存Paths
	MapSaveTypeMapDataArea  MapSaveType = 2 //保存成Area
)

type Map struct {
	MapName           string      `json:"map_name" bson:"map_name" binding:"required"`         //地图名 厂区-楼层@交通图名
	ProjectId         string      `json:"project_id" bson:"project_id" binding:"required"`     //厂区-楼层
	TrafficName       string      `json:"traffic_name" bson:"traffic_name" binding:"required"` //交通图名
	Type              MapSaveType `json:"type" bson:"type" binding:"required"`                 //地图保存类型
	MapData           MapMetaData `json:"-" bson:"map_data"`                                   //地图数据
	RawBase64         base64.Data `json:"-" bson:"raw"`                                        //地图原数据（xml base64）
	MapDataJsonString string      `json:"-" bson:"map_data_json"`                              //地图原数据（json）
}

func (Map) TableName() string {
	return "t_data_maps"
}

type MapSlice []*Map

func (a MapSlice) Len() int { // 重写 Len() 方法
	return len(a)
}
func (a MapSlice) Swap(i, j int) { // 重写 Swap() 方法
	a[i], a[j] = a[j], a[i]
}
func (a MapSlice) Less(i, j int) bool { // 重写 Less() 方法， 从大到小排序
	return a[j].MapName > a[i].MapName
}

type MapMetaData struct {
	MapInfo  MapInfo             `bson:"map_info"`
	NodesMap map[string]Node     `bson:"nodes_map"`  //点位集合
	Paths    map[string]Path     `bson:"paths"`      //线集合
	Areas    map[string]Area     `bson:"areas"`      //区域集合
	Obstacle map[string]Obstacle `bson:"obstacles"`  //障碍物集合
	MarkArea map[string]MarkArea `bson:"mark_areas"` //障碍物集合
	Links    map[string]Link     `bson:"links"`      //链接线集合
}

type MapInfo struct {
	MapName     string `bson:"map_name"`     //地图名称 地图唯一标志
	ProjectId   string `bson:"project_id"`   //厂区
	TrafficName string `bson:"traffic_name"` //楼层
}

// 处理地图适配前移车，绑定终点前一个点，且设置中转点
func (m *MapMetaData) HandleMapAdjustFrontAGV() {
	getConnectPaths := func(node Node) map[string]Path {
		res := make(map[string]Path, 0)
		for _, l := range m.Paths {
			if l.NodeA.Id == node.Id || l.NodeB.Id == node.Id {
				if _, find := res[l.PathId]; !find {
					res[l.PathId] = l
				}
			}
		}
		return res
	}
	for _, point := range m.NodesMap {
		// if point.Type == NodeStation && point.IntermediateNode != "" {
		if point.Type == NodeStation {
			for _, nodePath := range getConnectPaths(point) {
				if nodePath.NodeA.Type != NodeStation {
					nodePath.NodeA.BanNodes = append(nodePath.NodeA.BanNodes, point.Id)
					nodePath.NodeA.BanNodes = append(nodePath.NodeA.BanNodes, point.BanNodes...)
					nodePath.NodeA.IntermediateNode = point.IntermediateNode
					m.NodesMap[nodePath.NodeA.Id] = nodePath.NodeA
					//将当前工位点绑定路径前一个点
					point.BanNodes = append(point.BanNodes, nodePath.NodeA.Id)
					m.NodesMap[point.Id] = point

				} else if nodePath.NodeB.Type != NodeStation {
					nodePath.NodeB.BanNodes = append(nodePath.NodeB.BanNodes, point.Id)
					nodePath.NodeB.BanNodes = append(nodePath.NodeB.BanNodes, point.BanNodes...)
					nodePath.NodeB.IntermediateNode = point.IntermediateNode
					m.NodesMap[nodePath.NodeB.Id] = nodePath.NodeB
					//将当前工位点绑定路径前一个点
					point.BanNodes = append(point.BanNodes, nodePath.NodeB.Id)
					m.NodesMap[point.Id] = point

				}
			}
		}
	}
}
func (m *MapMetaData) HandleMapAdjustFrontAGV_1() {
	getConnectPaths := func(node Node) map[string]Path {
		res := make(map[string]Path, 0)
		for _, l := range node.ConnectPaths {
			path := m.Paths[l]
			res[path.PathId] = path
		}
		return res
	}
	getNodeFrontList := func(copiedNodeMap map[string]Node, node Node) []string {
		res := make([]string, 0)
		for _, l := range node.BanNodes {
			tmp := copiedNodeMap[l]
			res = append(res, tmp.StockFrontId)
		}
		return tools.RemoveDuplicateElement(res)
	}
	nodeIsWorkType := func(p Node) bool {
		if p.Type == NodeStation || p.Type == NodeStacker || p.Type == NodeConveyorLine || p.Type == NodeManipulator || p.Type == NodeIn || p.Type == NodeOut {
			return true
		}
		return false
	}
	bindFrontMap := make(map[string]string, 0)
	//深拷贝
	copiedNodeMap := make(map[string]Node)
	err123 := tools.DeepCopy(m.NodesMap, &copiedNodeMap)
	if err123 != nil {
		log.Panicf("m.NodesMap 深拷贝错误,错误信息:%s", err123.Error())
	}

	for _, point := range copiedNodeMap {
		if nodeIsWorkType(point) {
			for _, nodePath := range getConnectPaths(point) {
				connectNode := nodePath.GetAnother(point)
				if !nodeIsWorkType(connectNode) {
					connectNode.StockFrontFlag = true
					point.StockFrontId = connectNode.Id
					bindFrontMap[point.Id] = connectNode.Id
					bindFrontMap[connectNode.Id] = point.Id
					connectNode.BanNodes = append(connectNode.BanNodes, point.BanNodes...)
					copiedNodeMap[connectNode.Id] = connectNode
					copiedNodeMap[point.Id] = point
				}
			}
		}
	}

	for _, point := range copiedNodeMap {
		if nodeIsWorkType(point) {
			if connectNode, find := copiedNodeMap[point.StockFrontId]; find {
				connectNode.StockFrontFlag = true
				point.StockFrontId = connectNode.Id
				if point.LocationType == LocationGround {
					fronList := getNodeFrontList(copiedNodeMap, point)
					connectNode.BanNodes = append(connectNode.BanNodes, fronList...) //将当前前一点绑定绑定库位的前一个点
					point.BanNodes = append(point.BanNodes, fronList...)             //将当前工位点绑定绑定库位的前一个点
				} else {
					connectNode.BanNodes = append(connectNode.BanNodes, point.Id)
					//将当前工位点绑定路径前一个点
					point.BanNodes = append(point.BanNodes, connectNode.Id)
				}
				m.NodesMap[connectNode.Id] = connectNode
				// copiedNode := new(Node)
				// tools.DeepCopy(point, &copiedNode)
				// m.NodesMap[point.ID] = copiedNode
				m.NodesMap[point.Id] = point
			}
		}
	}
	//拷贝构造
	copiedNodeMap1 := make(map[string]Node)
	err2 := tools.DeepCopy(m.NodesMap, &copiedNodeMap1)
	if err2 != nil {
		log.Panicf("m.NodesMap 深拷贝错误,错误信息:%s", err2.Error())
	}
	//将货位点再次绑定关联货位点的钱一个点
	for _, point1 := range m.NodesMap {
		//为每个点添加绑定点
		if nodeIsWorkType(point1) || point1.StockFrontFlag {
			currentPoinBans := point1.BanNodes
			for _, value := range point1.BanNodes {
				if v, ok := bindFrontMap[value]; ok {
					if !tools.StringSliceIsContain(currentPoinBans, v) && v != point1.Id {
						currentPoinBans = append(currentPoinBans, v)
					}
				}
			}
			point1.BanNodes = currentPoinBans
			m.NodesMap[point1.Id] = point1
		}
	}
}

func (m *MapMetaData) MakeGlobalPath(sn, mapName string, angle int, target string, ids []string) *GlobalPath {
	gp := &GlobalPath{Sn: sn, MapName: mapName, Target: target}
	gp.EndBlocks = make([]string, 0)
	lastID := "0"
	first := true
	for _, id := range ids {
		if first {
			lastID = id
			first = false
			continue
		}
		pathItem, _ := m.GetPathBySrcAndTar(lastID, id)
		gp.Paths = append(gp.Paths, &TrafficPath{Path: *pathItem, PathBlocks: nil, SourceBlocks: nil, TargetBlocks: nil, RotateType: 0})
		lastID = id
	}

	return m.PathAddRotationType(gp, angle)
}

func (m *MapMetaData) PathAddRotationType(gp *GlobalPath, angle int) *GlobalPath {
	for i, path := range gp.Paths {
		line1Angle, line2Angle, line2BezierEndAngle := 0.0, 0.0, 0.0
		if i == 0 {
			line1Angle = float64(angle)
			line2Angle = m.GetLineAngle(&path.Path)
			if path.Path.Type == PathTypeBezier {
				line2Angle, line2BezierEndAngle = path.Path.BodyBezierThetaRad()
				line2Angle = line2Angle * 180.0 / math.Pi
				if line2Angle < 0 {
					line2Angle = line2Angle + 360
				}

			}
			//} else if i == len(gp.Paths)-1 {
			//	continue
		} else {
			line1Angle = m.GetLineAngle(&gp.Paths[i-1].Path)
			if gp.Paths[i-1].Path.Type == PathTypeBezier {
				_, line1Angle = gp.Paths[i-1].Path.BodyBezierThetaRad()
				line1Angle = line1Angle * 180.0 / math.Pi
				if line1Angle < 0 {
					line1Angle = line1Angle + 360
				}
			}
			line2Angle = m.GetLineAngle(&path.Path)
			if path.Path.Type == PathTypeBezier {
				line2Angle, line2BezierEndAngle = path.Path.BodyBezierThetaRad()
				line2Angle = line2Angle * 180.0 / math.Pi
				if line2Angle < 0 {
					line2Angle = line2Angle + 360
				}
			}
		}
		rotationType := m.GetRotationType(angles.Deg2Rad(line1Angle), angles.Deg2Rad(line2Angle))
		// gp.Paths[i].BodyDir = DirectionHead
		// if (gp.Paths[i].BodyDirec == HeadAB && gp.Paths[i].Dir == DirectionB2A) || (gp.Paths[i].BodyDirec == HeadBA && gp.Paths[i].Dir == DirectionA2B) {
		gp.Paths[i].BodyDir = gp.Paths[i].BodyDirec
		// }
		gp.Paths[i].RotateType = rotationType
		gp.Paths[i].RotateStartAng = int(line1Angle)
		gp.Paths[i].RotateEndAng = int(line2Angle)
		gp.Paths[i].PathEndAng = int(line2Angle)
		if path.Path.Type == PathTypeBezier {
			line2BezierEndAngle = line2BezierEndAngle * 180.0 / math.Pi
			if line2BezierEndAngle < 0 {
				line2BezierEndAngle = line2BezierEndAngle + 360
			}
			gp.Paths[i].PathEndAng = int(line2BezierEndAngle)
		}
	}
	return gp
}

//计算小车所在线的最近点
func (m *MapMetaData) NeareastOnlineSite(r *Robot, cfg *SystemConfig) string {
	x := r.Status.X
	y := r.Status.Y
	angle := r.Status.Angle
	angle_res := float64(cfg.RobotOnlineCfg.LineOffsetAngle) * math.Pi / 180
	line_res := float64(cfg.RobotOnlineCfg.LineOffsetLength)
	dis_res := float64(cfg.RobotOnlineCfg.LineOffsetSiteDis)
	min_dis := 9999
	online_id := "0"

	for _, p := range m.Paths {
		line2Angle := m.GetLineAngle(&p)
		dtheta := (float64(angle) - line2Angle) * math.Pi / 180
		if math.Abs(angles.NormalizeAngle(dtheta)) < angle_res {
			min_dis_tmp, online_id_tmp := p.DistanceByXY(x, y, angle, line_res, dis_res)
			if min_dis_tmp < min_dis {
				min_dis = min_dis_tmp
				online_id = online_id_tmp
				//log.Errorf("min_dis = %d online_id=%d", min_dis, online_id)
			}
		}
	}
	//log.Infof("智能上线 线距:%d 距离点:%d (配置:线距:%f 夹角:%f 点距离:%f)", min_dis, online_id, line_res, angle_res, dis_res)
	// fmt.Printf("智能上线 线距:%d 距离点:%s (配置:线距:%f 夹角:%f 点距离:%f)\n", min_dis, online_id, line_res, angle_res, dis_res)
	return online_id
}

//计算点到线段距离
func (path Path) DistanceByXY(x, y, angle int, line_res, dis_res float64) (int, string) {
	thetaA := math.Atan2((float64(path.NodeA.Y) - float64(y)), (float64(path.NodeA.X) - float64(x)))
	thetaB := math.Atan2((float64(path.NodeB.Y) - float64(y)), (float64(path.NodeB.X) - float64(x)))

	dthetaA := math.Abs(angles.NormalizeAngle(thetaA - float64(angle)*math.Pi/180))
	dthetaB := math.Abs(angles.NormalizeAngle(thetaB - float64(angle)*math.Pi/180))
	dis := 9999
	online_id := "0"
	if (dthetaA >= math.Pi/2 && dthetaB >= math.Pi/2) || (dthetaA <= math.Pi/2 && dthetaB <= math.Pi/2) {
		return dis, online_id
	}
	if path.NodeA.X == path.NodeB.X {
		line_dis := math.Abs(float64(x - path.NodeA.X))
		if line_dis < line_res {
			dis = int(line_dis)
		}
	} else {
		k := (float64(path.NodeB.Y) - float64(path.NodeA.Y)) / (float64(path.NodeB.X) - float64(path.NodeA.X))
		b := float64(path.NodeB.Y) - k*float64(path.NodeB.X)
		line_dis := math.Abs(k*float64(x)-float64(y)+b) / math.Sqrt(k*k+1)
		if line_dis < line_res {
			dis = int(line_dis)
		}
	}
	if dthetaA < math.Pi/2 && dthetaB > math.Pi/2 {
		if path.NodeA.DistanceByXY(x, y) < dis_res {
			online_id = path.NodeA.Id
		}
	} else {
		if path.NodeB.DistanceByXY(x, y) < dis_res {
			online_id = path.NodeB.Id
		}
	}
	return dis, online_id
}

func (m *MapMetaData) GetNeareastOnlineSiteAngle(r *Robot, site string) (float64, codes.Code) {
	node, find := m.NodesMap[site]
	if find {
		theta := math.Atan2((float64(node.Y) - float64(r.Status.Y)), (float64(node.X) - float64(r.Status.X)))
		return theta * 180 / math.Pi, nil
	}
	return 0, codes.ErrMapNodeNotFound
}

func (m *MapMetaData) GetLineAngle(path *Path) float64 {
	nodeA := path.NodeA
	nodeB := path.NodeB
	theta := 0.0
	if path.Dir == DirectionA2B {
		theta = math.Atan2((float64(nodeB.Y) - float64(nodeA.Y)), (float64(nodeB.X) - float64(nodeA.X)))

	} else {

		theta = math.Atan2((float64(nodeA.Y) - float64(nodeB.Y)), (float64(nodeA.X) - float64(nodeB.X)))
	}
	angle := theta * 180 / math.Pi

	if (path.Dir == DirectionA2B && path.BodyDirec == HeadBA) || (path.Dir == DirectionB2A && path.BodyDirec == HeadAB) {
		if angle > 180 {
			angle = angle - 180

		} else {
			angle = angle + 180
		}
	}
	if angle < 0 {
		angle = angle + 360
	}
	return angle
}

//line1Angle：起点线段角度 、line2Angle：终点线段角度
func (m *MapMetaData) GetRotationType(line1Angle, line2Angle float64) int {
	//返回值：0=不旋转、1=逆时针旋转、-1=顺时针旋转
	angle := angles.NormalizeAngle(line2Angle - line1Angle)
	if angle > angles.Deg2Rad(10) && angle <= angles.Deg2Rad(180) {
		return 1
	} else if angle < angles.Deg2Rad(-10) && angle >= angles.Deg2Rad(-180) {
		return -1
	}
	return 0
}

func (m *MapMetaData) GetPathBySrcAndTar(source, target string) (*Path, bool) {
	var newPath = new(Path)
	if p, find := m.Paths[source+"-"+target]; find {
		tools.Copy(p, newPath)
		return newPath, true
	}
	return nil, false
}

type NodeType uint8

const (
	NodePath           NodeType = 1  //路径点
	NodeTransitPoint   NodeType = 2  //中转点
	NodeStation        NodeType = 3  //货架点
	NodeIn             NodeType = 4  //入库点
	NodeOut            NodeType = 5  //出库点
	NodeWaiting        NodeType = 6  //待命点
	NodeCharge         NodeType = 7  //充电点
	NodeDoorFront      NodeType = 8  //门前点
	NodeAutoDoor       NodeType = 9  //自动门
	NodeElevator       NodeType = 10 //电梯
	NodeConveyorLine   NodeType = 11 //输送线
	NodeManipulator    NodeType = 12 //机械手
	NodeHoist          NodeType = 13 //提升机
	NodeStacker        NodeType = 14 //叠盘机
	NodeDiscRemover    NodeType = 15 //拆盘机
	NodeRgv            NodeType = 16 //穿梭车
	NodeAvoid          NodeType = 17 //避让点
	NodeSwitch         NodeType = 18 //地图切换点
	NodeGroundWeight   NodeType = 19 //地称点
	NodePlatformWeight NodeType = 20 //承重平台点

)

type Node struct {
	Name              string           `bson:"name"`                //名称
	Id                string           `bson:"id"`                  //点位
	Type              NodeType         `bson:"type"`                //类型
	LocationType      TaskLocationType `bson:"location_type"`       //库位类型
	X                 int              `bson:"x"`                   //X坐标
	Y                 int              `bson:"y"`                   //Y坐标
	DeviceID          string           `bson:"device_id"`           //设备ID
	BanNodes          []string         `bson:"ban_nodes"`           //关联绑定点位ID
	IntermediateNode  string           `bson:"intermediate_node"`   //中转点
	AssociatedMutexId string           `bson:"associated_mutex_id"` //关联的胡吃区域Id
	AssociatedBlocks  []string         `bson:"-"`                   //关联的点线集合
	ForkRotateFlag    int              `bson:"fork_rotate_flag"`    //货叉旋转标志位 0=不可旋转、1=可旋转
	BanDir            int              `bson:"ban_dir"`             //站点禁止方向(-1=无禁止方向，0-360=有进行方向)
	AllowAvoid        int              `bson:"allow_avoid"`         //允许避让(0=不允许、1=允许)
	ConnectPaths      []string         `bson:"connect_path"`        //该点连接的路径
	StockFrontFlag    bool             `bson:"stock_front_flag"`    //货位点前一个点标记
	StockFrontId      string           `bson:"stock_front_id"`      //货位点前一个点Id
	Reserve1          string           `bson:"reserve1"`            //预留一（计算货叉旋转点用，如：10001;10003）
	Reserve2          string           `bson:"reserve2"`            //预留二（计算目标点是否需要严格按照顺序 如：1严格按照顺序）
}

type PriorityNode struct {
	Priority int
	Node     Node
}
type NodeD struct {
	CurrentNode Node
	Cost        float64
}

//计算两点的直线距离
func (node Node) DistanceByXY(x, y int) float64 {
	return math.Sqrt(float64(node.X-x)*float64(node.X-x) + float64(node.Y-y)*float64(node.Y-y))
}

//计算两点的直线距离
func (node Node) Distance(n Node) float64 {
	return node.DistanceByXY(n.X, n.Y)
}

//判断点位是否在站点锁定中
func (node Node) In(nodeMapName string, lockedSites []LockedSite) bool {
	if lockedSites == nil || len(lockedSites) < 1 {
		return false
	}
	for _, lockedSite := range lockedSites {
		if lockedSite.MapName == nodeMapName && lockedSite.SiteId == node.Id {
			return true
		}
	}
	return false
}

// Dimensions implement kd tree point interface
func (node *Node) Dimensions() int {
	return 2
}

// Dimension implement kd tree point interface
func (node *Node) Dimension(i int) float64 {
	if i == 0 {
		return float64(node.X)
	}
	return float64(node.Y)
}

// String ...
func (p *Node) String() string {
	return fmt.Sprintf("{%.2f %.2f}", p.X, p.Y)
}

type PathDirection uint8

const (
	DirectionAB  PathDirection = 0
	DirectionA2B PathDirection = 1
	DirectionB2A PathDirection = 2
)

type BodyDirection uint8

const (
	Head   BodyDirection = 0 //A->B的路，车头从A->B,  B->A的路，车头B->A
	HeadAB BodyDirection = 1 //车头从A指向B
	HeadBA BodyDirection = 2 //车头从B指向A
)

type BodyDirectionPath uint8

const (
	DirectionHT    BodyDirectionPath = 0 //初始值，无含义
	DirectionHead  BodyDirectionPath = 1 //车头前进
	DirectionTail  BodyDirectionPath = 2 //车尾前进
	DirectionLeft  BodyDirectionPath = 3 //车左边前进
	DirectionRight BodyDirectionPath = 4 //车右边前进
)

type PathType uint8

const (
	PathTypeLine   PathType = 1 //直线
	PathTypeArc    PathType = 2 //圆弧
	PathTypeBezier PathType = 3 //贝塞尔曲线
)

type Path struct {
	PathId        string        `bson:"id"`               //路径编号
	Id            int           `bson:"id_int"`           //路径编号
	Name          string        `bson:"name"`             //路径名
	Type          PathType      `bson:"type"`             //路径类型
	Dir           PathDirection `bson:"dir"`              //路径朝向
	BodyDirec     BodyDirection `bson:"body_dir"`         //路径车身方向
	NodeA         Node          `bson:"node_a"`           //路径NodeA
	NodeB         Node          `bson:"node_b"`           //路径NodeB
	NodeAControlX float64       `bson:"node_a_control_x"` //NodeA控制点x (贝塞尔)
	NodeAControlY float64       `bson:"node_a_control_y"` //NodeA控制点y (贝塞尔)
	NodeBControlX float64       `bson:"node_b_control_x"` //NodeB控制点x (贝塞尔)
	NodeBControlY float64       `bson:"node_b_control_y"` //NodeB控制点y (贝塞尔)
	PathLength    float64       `bson:"path_length"`      //路径长度
	PathVel       float64       `bson:"path_vel"`         //路径长度
	AllowBack     int           `bson:"allow_back"`       //允许后退(0=不允许、1=允许)
	Reserve1      string        `bson:"reserve1"`         //允许后退(0=不允许、1=允许)
	Reserve2      string        `bson:"reserve2"`         //允许后退(0=不允许、1=允许)
}
type Link struct {
	LinkId   string        `bson:"id"`       //路径编号
	Id       int           `bson:"id_int"`   //路径编号
	Name     string        `bson:"name"`     //路径名
	Type     PathType      `bson:"type"`     //路径类型
	Dir      PathDirection `bson:"dir"`      //路径朝向
	NodeA    Node          `bson:"node_a"`   //路径NodeA
	NodeB    Node          `bson:"node_b"`   //路径NodeB
	Reserve1 string        `bson:"reserve1"` //允许后退(0=不允许、1=允许)
	Reserve2 string        `bson:"reserve2"` //允许后退(0=不允许、1=允许)
}

type TargetCarPose struct {
	CarBodyRad   float64
	PathID       string
	TargetSiteId string
}

//获取路径起点
func (p *Path) GetSource() Node {
	if p.Dir == DirectionA2B {
		return p.NodeA
	}
	if p.Dir == DirectionB2A {
		return p.NodeB
	}
	log.Errorf("Path:%+v GetSource Error", p)
	panic("not hear")
}

//获取路径终点
func (p *Path) GetTarget() Node {
	if p.Dir == DirectionA2B {
		return p.NodeB
	}
	if p.Dir == DirectionB2A {
		return p.NodeA
	}
	log.Errorf("Path:%+v GetTarget Error", p)
	panic("not hear")
}

func (p *Path) GetAnother(node Node) Node {
	if p.NodeA.Id == node.Id {
		return p.NodeB
	}
	if p.NodeB.Id == node.Id {
		return p.NodeA
	}
	log.Errorf("Path:%+v GetAnother Error", p)
	panic("not hear")
}

func (p *Path) Length() float64 {
	dis := math.Sqrt(math.Pow(float64(p.NodeA.X-p.NodeB.X), 2) + math.Pow(float64(p.NodeA.Y-p.NodeB.Y), 2))
	return dis
}
func (p *Path) GetType() PathType {
	return p.Type
}

//BodyThetaRad path的车身方向，rad
func (p *Path) BodyThetaRad() float64 {
	switch p.BodyDirec {
	case Head:
		switch p.Dir {
		case DirectionA2B:
			return math.Atan2(float64(p.NodeB.Y-p.NodeA.Y), float64(p.NodeB.X-p.NodeA.X))
		case DirectionB2A:
			return math.Atan2(float64(p.NodeA.Y-p.NodeB.Y), float64(p.NodeA.X-p.NodeB.X))
		default:
			panic("calculate theta error,dir not match")
		}
	case HeadAB:
		return math.Atan2(float64(p.NodeB.Y-p.NodeA.Y), float64(p.NodeB.X-p.NodeA.X))
	case HeadBA:
		return math.Atan2(float64(p.NodeA.Y-p.NodeB.Y), float64(p.NodeA.X-p.NodeB.X))
	}
	panic(fmt.Sprintf("path %s calculate theta error,body dir not match", p.PathId))
}
func (p *Path) BodyBezierThetaRad() (start_ang, end_ang float64) {
	switch p.BodyDirec {
	case Head:
		switch p.Dir {
		case DirectionA2B:
			return math.Atan2(p.NodeAControlY-float64(p.NodeA.Y), p.NodeAControlX-float64(p.NodeA.X)), math.Atan2(float64(p.NodeB.Y)-p.NodeBControlY, float64(p.NodeB.X)-p.NodeBControlX)
		case DirectionB2A:
			return math.Atan2(p.NodeBControlY-float64(p.NodeB.Y), p.NodeBControlX-float64(p.NodeB.X)), math.Atan2(float64(p.NodeA.Y)-p.NodeAControlY, float64(p.NodeA.X)-p.NodeAControlX)
		default:
			panic("calculate theta error,dir not match")
		}
	case HeadAB:
		if p.Dir == DirectionA2B {
			return math.Atan2(p.NodeAControlY-float64(p.NodeA.Y), p.NodeAControlX-float64(p.NodeA.X)), math.Atan2(float64(p.NodeB.Y)-p.NodeBControlY, float64(p.NodeB.X)-p.NodeBControlX)
		} else {
			return math.Atan2(float64(p.NodeB.Y)-p.NodeBControlY, float64(p.NodeB.X)-p.NodeBControlX), math.Atan2(p.NodeAControlY-float64(p.NodeA.Y), p.NodeAControlX-float64(p.NodeA.X))
		}

	case HeadBA:
		if p.Dir == DirectionB2A {
			return math.Atan2(p.NodeBControlY-float64(p.NodeB.Y), p.NodeBControlX-float64(p.NodeB.X)), math.Atan2(float64(p.NodeA.Y)-p.NodeAControlY, float64(p.NodeA.X)-p.NodeAControlX)
		} else {
			return math.Atan2(float64(p.NodeA.Y)-p.NodeAControlY, float64(p.NodeA.X)-p.NodeAControlX), math.Atan2(p.NodeBControlY-float64(p.NodeB.Y), p.NodeBControlX-float64(p.NodeB.X))
		}
	}
	panic("calculate theta error,body dir not match")
}

func (p *Path) String() string {
	switch p.Dir {
	case DirectionAB:
		return fmt.Sprintf("%s<%s <-> %s>", p.PathId, p.NodeA.Id, p.NodeB.Id)
	case DirectionA2B:
		return fmt.Sprintf("%s<%s -> %sd>", p.PathId, p.NodeA.Id, p.NodeB.Id)
	case DirectionB2A:
		return fmt.Sprintf("%s<%s -> %s>", p.PathId, p.NodeB.Id, p.NodeA.Id)
	}
	return ""
}

//地图区域
type Area struct {
	Name            string   `bson:"name"`              //名称
	Id              string   `bson:"id"`                //点位
	Type            AreaType `bson:"type"`              //类型
	BanAgvIds       []string `bson:"ban_agv_ids"`       //禁用AgvId集合
	EdgePoints      []Pose2  `bson:"edge_points"`       //绘制区域边界点位坐标集合
	ContainDotIds   []string `bson:"contain_dot_ids"`   //区域包含的点位ID集合
	ContainPathIds  []Path   `bson:"path_ids"`          //区域包含的点位ID集合
	FrontLookPoints []string `bson:"front_look_points"` //前瞻点包含的点位ID集合
	AreaWork        bool     `bson:"area_work"`         //区域生效
	LockMethod      int      `bson:"positive_way"`      //潮汐区域正向 0 均不锁定 1正向锁定 2 反向锁定 3 两侧锁定
	AgvList         []string `bson:"agv_list"`          //区域内所有的agv
	AlowRotate      int      `bson:"alow_rotate"`       //+允许旋转
	IsApply         int      `bson:"is_apply"`          //+0:必须申请 1:不需要申请
	RobotLimitCount uint     `bson:"robot_limit_count"` //小车限制数量
}

type AreaType uint8
type ObstacleType uint8

const (
	AreaTypeBan                AreaType = 1  //禁行区域
	AreaTypeMutex              AreaType = 2  //互斥区域
	AreaTypeInteractive        AreaType = 3  //第三方交互区域
	AreaTypeRobotLimitCount    AreaType = 4  //小车限数区域
	AreaTypeTide               AreaType = 5  //潮汐区域
	AreaTypeDeadWay            AreaType = 6  //死胡同区域
	AreaTypeAllowPathMutex     AreaType = 7  //允许路线对冲区域
	AreaTypeDoorArea           AreaType = 8  //门区域
	AreaTypeNoParkingArea      AreaType = 9  //禁停区域
	AreaTypeTaskLimitRobotArea AreaType = 10 //任务限定

	ObstacleTypeWall         ObstacleType = 1 //障碍物墙
	ObstacleTypeColumn       ObstacleType = 2 //障碍物柱子
	ObstacleTypeGoodsShelves ObstacleType = 3 //障碍物货架
	ObstacleTypeHopperCar    ObstacleType = 4 //障碍物料车
)

//障碍物
type Obstacle struct {
	Name       string       `bson:"name"`        //名称
	Id         string       `bson:"id"`          //点位
	Type       ObstacleType `bson:"type"`        //类型
	EdgePoints PoseArray    `bson:"edge_points"` //绘制区域边界点位坐标集合
}

type MarkArea struct {
	Name       string    `bson:"name"`        //名称
	Id         string    `bson:"id"`          //点位
	Encode     string    `bson:"encode"`      //区域编码
	EdgePoints PoseArray `bson:"edge_points"` //绘制区域边界点位坐标集合

}

//Slam地图
type SlamMap struct {
	ProjectId   string  `json:"project_id"`              //工程名称
	MapOffsetX  float64 `json:"map_origin_offset_x"`     //工程偏置X
	MapOffsetY  float64 `json:"map_origin_offset_y"`     //工程偏置Y
	MapOffsetR  float64 `json:"map_origin_offset_theta"` //工程偏置R
	ProjectSlam struct {
		Height    int     `json:"height"`     //高度
		Width     int     `json:"width"`      //宽度
		OriginX   float64 `json:"origin_x"`   //原点X坐标
		OriginY   float64 `json:"origin_y"`   //原点Y坐标
		OriginAng float64 `json:"origin_ang"` //原点角度坐标
		MapData   string  `json:"map_data"`   //slam图base64转码
		MapInfo   string  `json:"map_info"`   //json文件base64转码
	} `json:"project_slam"` //地图数据
	TrafficList []struct {
		MapName string `json:"map_name"` //交通图名
		MapData string `json:"map_data"` //交通图base64转码
	} `json:"traffic_map_list"` //地图数据
}
